/*
 * OREKIT-X
 * Copyright 2002-2008 CS Communication & Systemes
 * 
 * Parts of this software package have been licensed to CS
 * Communication & Systemes (CS) under one or more contributor license
 * agreements.  See the NOTICE file distributed with this work for
 * additional information.
 *  
 * This is an experimental copy of OREKIT from www.orekit.org.
 * Please use the original OREKIT from orekit.org for normal work
 * unrelated to this research project.
 * 
 * Licensed under the Apache License, Version 2.0 (the "License"); you
 * may not use this file except in compliance with the License.  You
 * may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
 * implied.  See the License for the specific language governing
 * permissions and limitations under the License.
 */
package ore.frames;

import ore.Frame;
import ore.PositionVelocity;
import ore.Transform;
import ore.bodies.BodyShape;
import ore.bodies.GeodeticPoint;
import ore.errors.OrekitException;
import ore.time.AbsoluteDate;

import org.apache.commons.math.geometry.Rotation;
import org.apache.commons.math.geometry.Vector3D;

/** 
 * The topocentric frame is anchored to a position on the surface of a
 * body shape.  The parent frame is the frame of the body.  The frame
 * defines the orthogonal set of directions: East, North, and Zenith;
 * with their inverses: West, South, and Nadir.
 * 
 * @author V&eacute;ronique Pommier-Maurussane
 */
public class TopocentricFrame
    extends ore.Frame
{

    /** 
     * Point where the topocentric frame is defined.
     */
    public final GeodeticPoint point;


    /** 
     * @param parentShape Body shape on which the local point is defined
     * @param point Local surface point where topocentric frame is anchored
     * @param name Frame name
     */
    public TopocentricFrame(BodyShape parentShape, GeodeticPoint point, String name) {
        super(parentShape.getBodyFrame(), null, name);

        this.point = point;
        /*
         * Build transform from body centered frame to topocentric frame
         */
        Transform translation;
        Transform rotation;
        /*
         * Translation from body center to geodetic point
         */
        translation = new Transform(parentShape.transform(point).negate());
        {
            Vector3D Xtopo = point.getEast();
            Vector3D Ztopo = point.getZenith();
            rotation = new Transform(new Rotation(Xtopo, Ztopo, Vector3D.PLUS_I, Vector3D.PLUS_K),
                                     Vector3D.ZERO);
        }
        this.setTransform(new Transform(translation, rotation));
    }


    /** 
     * The zenith direction is defined as the normal to local
     * horizontal plane, and opposes the nadir direction.
     * 
     * @return Unit vector in the zenith direction, expressed in the
     * parent (body) frame.
     */
    public Vector3D getZenith() {
        return this.point.getZenith();
    }
    /** 
     * The nadir direction is the opposite of zenith direction.
     * 
     * @return unit vector in the nadir direction, expressed in the
     * parent (body) frame.
     */
    public Vector3D getNadir() {
        return this.point.getNadir();
    }
    /** 
     * The north direction is defined in the horizontal plane (normal
     * to zenith direction) and following the local meridian.
     * 
     * @return Unit vector in the north direction, expressed in the
     * parent (body) frame.
     */
    public Vector3D getNorth() {
        return this.point.getNorth();
    }
    /** 
     * @return Unit vector in the south direction, expressed in the
     * parent (body) frame.
     */
    public Vector3D getSouth() {
        return this.point.getSouth();
    }
    /** 
     * The east direction is defined in the horizontal plane to
     * complete orthogonal set: East, North, Zenith.
     * 
     * @return Unit vector in the east direction, expressed in the
     * parent (body) frame.
     */
    public Vector3D getEast() {
        return this.point.getEast();
    }
    /** 
     * The west direction is the opposite of east direction.
     * 
     * @return Unit vector in the west direction, expressed in the
     * parent (body) frame.
     */
    public Vector3D getWest() {
        return this.point.getWest();
    }

    /** 
     * Compute the elevation angle for a point with respect to the
     * local point.
     * 
     * Elevation is the angle between the local horizontal and the
     * direction from local point to given point, or PI/2 minus the
     * angle between the zenith and the given point direction.
     * 
     * @param extPoint Point for which elevation shall be computed
     * @param frame Frame in which the point is defined
     * @param date Computation date
     * 
     * @return Elevation angle in radians
     * 
     * @exception OrekitException Frame transformations cannot be determined
     */
    public double getElevation(Vector3D extPoint, Frame frame, AbsoluteDate date)
        throws OrekitException
    {
        /*
         * Transform given point from given frame to topocentric frame
         */
        final Transform t = frame.getTransformTo(this, date);
        final Vector3D extPointTopo = t.transformPosition(extPoint);
        /*
         * Elevation angle is PI/2 minus the angle between the zenith and
         * the given point direction
         */
        return Math.asin(extPointTopo.normalize().getZ());
    }
    /** 
     * Compute the azimuth of a point with respect to the topocentric
     * frame center point.
     * 
     * The azimuth is the angle between the North direction at local
     * point and the projection in local horizontal plane of the
     * direction from local point to given point. Azimuth angles are
     * counted clockwise, i.e positive towards the East.
     * 
     * @param extPoint point for which elevation shall be computed
     * @param frame frame in which the point is defined
     * @param date computation date
     * @return azimuth of the point
     * @exception OrekitException if frames transformations cannot be computed
     */
    public double getAzimuth(Vector3D extPoint, Frame frame, AbsoluteDate date)
        throws OrekitException
    {
        /*
         * Transform given point from given frame to topocentric frame
         */
        Transform t = getTransformTo(frame, date).getInverse();
        Vector3D extPointTopo = t.transformPosition(extPoint);
        /*
         * Compute azimuth
         */
        double azimuth = Math.atan2(extPointTopo.getX(), extPointTopo.getY());
        if (azimuth < 0.0) 
            azimuth += TWO_PI;

        return azimuth;
    }
    /** 
     * Compute the range (distance) of a point with respect to the
     * topocentric frame center point.
     * 
     * @param extPoint Point for which range shall be computed
     * @param frame Frame in which the point is defined
     * @param date Computation date
     * 
     * @return Range (distance) of the point
     * @exception OrekitException if frames transformations cannot be computed
     */
    public double getRange(Vector3D extPoint, Frame frame, AbsoluteDate date)
        throws OrekitException
    {
        /*
         * Transform given point from given frame to topocentric frame
         */
        final Transform t = getTransformTo(frame, date).getInverse();
        final Vector3D extPointTopo = t.transformPosition(extPoint);
        /*
         * Compute range
         */
        return extPointTopo.getNorm();
    }
    /** 
     * @param extPV Position and velocity for which range rate shall be computed
     * @param frame Frame in which the PV is defined
     * @param date Date at which PV is defined
     * 
     * @return Range rate of the point (positive if point departs from frame)
     * 
     * @exception OrekitException if frames transformations cannot be computed
     */
    public double getRangeRate(PositionVelocity extPV, Frame frame, AbsoluteDate date)
        throws OrekitException
    {
        /*
         * Transform PV into frame
         */
        final Transform t = frame.getTransformTo(this, date);
        final PositionVelocity extPVTopo = t.transformPositionVelocity(extPV);
        /*
         * Relative rate along the line of sight
         */
        return (Vector3D.dotProduct(extPVTopo.getPosition(), extPVTopo.getVelocity()) /
                extPVTopo.getPosition().getNorm());
    }
}
